#include <iostream>
#include <vector>
#include <string>
#include <fstream>

#include <cv.h>
#include <highgui.h>

#include "../Common/Camera/PGRCamera.h"
#include "../Common/Utils/utils.h"
#include "../Common/Calibration/wVector.h"
#include "../Common/Calibration/SelfCalibrator.h"

double width = 640;
double height = 480;
int GRID_X = 9;
int GRID_Y = (GRID_X-1) / 4 * 3 + 1;
windage::SelfCalibrator* calibrator;
const char* SELF_CALIBRATION_FILE = "selfcalibration.txt";

int selectedPoint = -1;
bool rbuttondown = false;
windage::Vector2 outputPoint;

void mouseCallback(int event, int x, int y, int flags, void* param)
{
	std::vector<windage::Vector2>* adjustmentPoints = calibrator->GetAdjustmentPoints();

	// normalized
	windage::Vector2 point(x, y);
	point.x /= width;
	point.y /= height;
	switch(event)
	{
	case CV_EVENT_RBUTTONDOWN:
		{
			outputPoint = calibrator->ConvertPoint(point);
			outputPoint.x *= width;
			outputPoint.y *= height;
			rbuttondown = true;
		}
		break;
	case CV_EVENT_RBUTTONUP:
		rbuttondown = false;
		break;
	case CV_EVENT_LBUTTONDOWN:
		{
			double minDistance = 1.0e10;
			for(unsigned int i=0; i<adjustmentPoints->size(); i++)
			{
				double distance = point.getDistance((*adjustmentPoints)[i]);
				if(minDistance > distance)
				{
					minDistance = distance;
					selectedPoint = i;
				}
			}
		}
		break;
	case CV_EVENT_LBUTTONUP:
		selectedPoint = -1;
		break;
	case CV_EVENT_MOUSEMOVE:
		{
			if(rbuttondown)
			{
				outputPoint = calibrator->ConvertPoint(point);
				outputPoint.x *= width;
				outputPoint.y *= height;
			}
			if(selectedPoint >= 0)
				(*adjustmentPoints)[selectedPoint] = point;
		}
		break;
	}
}

void main()
{
	cvNamedWindow("calibration points");
	cvSetMouseCallback("calibration points", mouseCallback);
		
	CPGRCamera* camera = new CPGRCamera();
	camera->open();
	camera->start();
	camera->update();
	
	IplImage* inputImage = camera->GetIPLImage();
	CvSize imageSize = cvGetSize(inputImage);
	IplImage* resultImage = cvCreateImage(imageSize, IPL_DEPTH_8U, 3);

	width = (double)imageSize.width;
	height = (double)imageSize.height;

	calibrator = new windage::SelfCalibrator(GRID_X, GRID_Y);
	
	bool processing = true;
	while(processing)
	{
		camera->update();
		inputImage = camera->GetIPLImage();
		cvCvtColor(inputImage, resultImage, CV_GRAY2BGR);

		// draw conver point
		cvCircle(resultImage, cvPoint((int)outputPoint.x, (int)outputPoint.y), 10, CV_RGB(0, 255, 0), CV_FILLED);

		// draw control poitns
		std::vector<windage::Vector2>* adjustmentPoints = calibrator->GetAdjustmentPoints();
		std::vector<windage::Vector2>* staticPoints = calibrator->GetStaticPoints();
		const int CROSS_SIZE = 5;
		if(selectedPoint >= 0)
		{
			windage::Vector2 point = (*adjustmentPoints)[selectedPoint];
			point.x *= width;
			point.y *= height;
			cvCircle(resultImage, cvPoint((int)point.x, (int)point.y), CROSS_SIZE*2, CV_RGB(255, 255, 0), CV_FILLED);
		}
		
		int pointCount = adjustmentPoints->size();
		for(int i=0; i<pointCount; i++)
		{
			windage::Vector2 point = (*adjustmentPoints)[i];
			point.x *= width;
			point.y *= height;

			double r = 255.0 * (((double)pointCount - (double)i) / (double)pointCount);
			double g = 0.0;
			double b = 255.0 * ((double)i / (double)pointCount);

			cvLine(resultImage, cvPoint((int)point.x-CROSS_SIZE, (int)point.y), cvPoint((int)point.x+CROSS_SIZE, (int)point.y), CV_RGB(255, 255, 255), 5);
			cvLine(resultImage, cvPoint((int)point.x, (int)point.y-CROSS_SIZE), cvPoint((int)point.x, (int)point.y+CROSS_SIZE), CV_RGB(255, 255, 255), 5);
			cvLine(resultImage, cvPoint((int)point.x-CROSS_SIZE, (int)point.y), cvPoint((int)point.x+CROSS_SIZE, (int)point.y), CV_RGB(r, g, b), 2);
			cvLine(resultImage, cvPoint((int)point.x, (int)point.y-CROSS_SIZE), cvPoint((int)point.x, (int)point.y+CROSS_SIZE), CV_RGB(r, g, b), 2);
//*
			windage::Vector2 stPoint = (*staticPoints)[i];
			stPoint.x *= width;
			stPoint.y *= height;
			cvLine(resultImage, cvPoint((int)point.x, (int)point.y), cvPoint((int)stPoint.x, (int)stPoint.y), CV_RGB(r, g, b), 1);
//*/
		}

		char message[100];
		sprintf_s(message, "press 'U' to update pose from control points");
		DrawTextToImage(resultImage, cvPoint(10, 15), message);
		sprintf_s(message, "press 'S' to save current pose");
		DrawTextToImage(resultImage, cvPoint(10, 30), message);
		sprintf_s(message, "press 'L' to load control points position");
		DrawTextToImage(resultImage, cvPoint(10, 45), message);
		sprintf_s(message, "mouse 'Left button' to tweak control points");
		DrawTextToImage(resultImage, cvPoint(10, 60), message);
		sprintf_s(message, "mouse 'Right button' to test coordinate conversion");
		DrawTextToImage(resultImage, cvPoint(10, 75), message);

		cvShowImage("calibration points", resultImage);
		char ch = cvWaitKey(1);
		switch(ch)
		{
		case 27:
		case 'q':
		case 'Q':
			processing = false;
			break;
		case 'u':
		case 'U':
			calibrator->UpdateHomography();
			break;
		case 's':
		case 'S':
			std::cout << "save points" << std::endl;
			{
				std::ofstream ex(SELF_CALIBRATION_FILE);
				std::vector<windage::Vector2>* adjustmentPoints = calibrator->GetAdjustmentPoints();

				ex << calibrator->GetGridSize().width << " " << calibrator->GetGridSize().height << std::endl;
				for(unsigned int i=0; i<adjustmentPoints->size(); i++)
				{
					ex << (*adjustmentPoints)[i].x << " " << (*adjustmentPoints)[i].y << std::endl;
				}
			}
			break;
		case 'l':
		case 'L':
			std::cout << "load points" << std::endl;
			{
				std::ifstream in(SELF_CALIBRATION_FILE);
				in >> GRID_X >> GRID_Y;
				calibrator->SetGridSize(GRID_X, GRID_Y);
				std::vector<windage::Vector2>* adjustmentPoints = calibrator->GetAdjustmentPoints();
				for(unsigned int i=0; i<adjustmentPoints->size(); i++)
				{
					in >> (*adjustmentPoints)[i].x >> (*adjustmentPoints)[i].y;
				}
				calibrator->UpdateHomography();
			}
		}
	}

	camera->stop();
	camera->close();
	delete camera;

	cvDestroyAllWindows();
}